import mujoco
import mujoco.viewer as viewer
from threading import Thread
import os
from L1_laser.sensor.sensor import Sensor
import config as cfg

class RunMujoco():
    def __init__(self, xml):
        self.xml = xml
        # 加载模型
        if not os.path.exists(self.xml):
            raise FileNotFoundError(f"XML file not found at {self.xml}")
        self.model = mujoco.MjModel.from_xml_path(self.xml)
        self.data = mujoco.MjData(self.model)
        self.sensor = Sensor(self.model, self.data)
        self.target_vel_control()
 
    def run(self):
        simulation_thread = Thread(target=self.__run_simulation)
        simulation_thread.start()
        
    # 初始化仿真
    def __run_simulation(self):
        with viewer.launch_passive(self.model, self.data) as v:
            print("Simulation running. Press Ctrl+C to exit.")
            while v.is_running():
                mujoco.mj_step(self.model, self.data)
                v.sync()

    # 获取激光雷达数据，偏航角和距离
    def get_laser_sensor_data(self):
        return self.sensor.get_laser_sensor_data()
    # 获取激光雷达旋转速度
    def get_laser_sensor_vel(self):
        return self.sensor.get_laser_sensor_vel()
        
    # 控制目标速度
    def target_vel_control(self):
        self.target_vel = cfg.ROTATE_VEL
        self.joint_name = "L1_laser_joint"
        self.joint_id = mujoco.mj_name2id(
            self.model, mujoco.mjtObj.mjOBJ_JOINT, self.joint_name
        )
        self.dof_adr = self.model.jnt_dofadr[self.joint_id]
        self.data.qvel[self.dof_adr] = self.target_vel
        